之前一直想把vrep和matlab的robotics工具箱做个配合，因为工具箱的一些功能比vrep方便一些。最近有些空闲时间来试一试。

%三、机器人的逆运动学
%3.1 使用解析解
%加载KR5模型
mdl_KR5
%显示机器人的参数
KR5
%可视化机械臂
KR5.plot(KR5.theta)
%让机器人的每个关节运动一定的角度到达某个位姿
qn=[0 0 pi/4 0 pi/6 pi/3]
KR5.plot(qn)
%获得T60
T0=KR5.fkine(qn)
T0 = 
    0.1294   -0.2241   -0.9659    0.3154
   -0.8660   -0.5000         0         0
   -0.4830    0.8365   -0.2588    -0.153
         0         0         0         1
%以T为已知条件，使用解析解求解相应的关节的旋转角度
q1=KR5.ikine6s(T)
%虽然返回的关节角不同，但是都能够使机械臂到达同一个位姿
q1 =
   3.141592653589793  -3.322643350613794   3.177467065619158   3.141592653589793   1.525922048049113   1.047197551196598
 
T1=KR5.fkine(q1)
T1 = 
    0.1294   -0.2241   -0.9659    0.3154
   -0.8660   -0.5000         0         0
   -0.4830    0.8365   -0.2588    -0.153
         0         0         0         1
%可以看出T0和T1相等 虽然二者的configuration不同
KR5.plot(q1)

%因为存在多解，可以通过一定的配置来获得唯一的解
q = R.IKINE6S(T, config) as above but specifies the configuration of the arm in the
form of a string containing one or more of the configuration codes:
‘l’ arm to the left (default)
‘r’ arm to the right
‘u’ elbow up (default)
‘d’ elbow down
‘n’ wrist not flipped (default)
‘f’ wrist flipped (rotated by 180 deg)
%指定唯一的解 
q2=KR5.ikine6s(T0,'run')
KR5.plot(q2)
% q = R.ikine(T) 如果 T是一条轨迹的信息，也能返回相应的关节值
If T represents a trajectory (4 × 4 × M) then the inverse kinematics is computed for
all M poses resulting in q (M × N) with each row representing the joint angles at the
corresponding pose.

 可以看出虽然机械臂的configuration不同但是末端的位姿是相同的。

%3.2使用数值解
%以puma560为例
%加载puma560模型
mdl_puma560;
p560
p560.n
qn=[0 pi/4 pi 0 pi/4 0]
T0=p560.fkine(qn)
p560.plot(qn)
%使用数值解 解逆运动学 可以看出qn和q1不等 
q1=p560.ikine(T0)

q1 =
  -0.000000000025218  -0.833533062705068   0.093955832614287   0.000000000039999  -0.831219096701711  -0.000000000028497

   ikine函数可选项比较多，具体的还是要看需求参考文档。

Options
‘ilimit’, L maximum number of iterations (default 500)
‘rlimit’, L maximum number of consecutive step rejections (default 100)
‘tol’, T final error tolerance (default 1e-10)
‘lambda’, L initial value of lambda (default 0.1)
‘lambdamin’, M minimum allowable value of lambda (default 0)
‘quiet’ be quiet
‘verbose’ be verbose
‘mask’, M mask vector (6 × 1) that correspond to translation in X, Y and Z, and rotation about
X, Y and Z respectively.
‘q0’, q initial joint configuration (default all zeros)
‘search’ search over all configurations
‘slimit’, L maximum number of search attempts (default 100)
‘transpose’, A use Jacobian transpose with step size A, rather than Levenberg-Marquadt