s2d.m

针对移动机械臂运动学

function d = s2d (matrix,q,o)
%S2D(Matrix, qrand(), option) Symbolic values (sym) to numeric values (double) 
%    1 - D0 = s2d(M, qrand())
%    2 - D0 = s2d(M, qrand(), 'var')

%July ~ December 2005, by Reinaldo M. do Nascimento

if nargin<2,error('s2d (matrix,qrand,optional)');end
if nargin==2 | o~='var'
         if isa(q.q,'sym')
                d=subs(matrix,[q.robot.chain.links,q.robot.chain.variables],[q.robot.chain.linksValues,q.q]);
         else
                d=double(subs(matrix,[q.robot.chain.links,q.robot.chain.variables],[q.robot.chain.linksValues,q.q]));
         end
else
         d=subs(matrix,q.robot.chain.variables,q.q);
end