Can I do a kinematics redundancy by using inverse kinematics?

Are there any ways to do Inverse kinematic and get something like this?

https://www.youtube.com/watch?v=sZYBC8Lrmdo&ab_channel=KUKA-Robots%26Automation

what constrait do I need to add?

I tried to do something like this, but it doesn't well succeed

lbr = importrobot('iiwa14.urdf'); 
lbr.DataFormat = 'row';
initialguess = lbr.homeConfiguration;
gripper = 'iiwa_link_ee_kuka';
link1 = 'iiwa_link_1';

count = 1;
gik = generalizedInverseKinematics('RigidBodyTree',lbr,'ConstraintInputs',{'pose','orientation','joint'});
poseConst = constraintPoseTarget(gripper);
poseConst.TargetTransform = trvec2tform([0.5 0.5 0.4])*eul2tform([0 0 -pi]);

jointConst = constraintJointBounds(lbr);
maxJointChange = deg2rad(10);

orientationConst = constraintOrientationTarget(link1);
min = -70;
max = 70;

for i = min:10:max
   if i == min
       jointConst.Weights = zeros(size(jointConst.Weights));
       orientationConst.TargetOrientation = eul2quat([deg2rad(i) 0 0]);
       qs(count,:) = gik(initialguess,poseConst,orientationConst,jointConst);
       initialguess = qs(count,:);
       count = count+1;
   else
       jointConst.Weights = ones(size(jointConst.Weights))*0.25;
       jointConst.Bounds = [qs(count-1,:)' - maxJointChange, qs(count-1,:)' + maxJointChange];
       
       orientationConst.TargetOrientation = eul2quat([deg2rad(i) 0 0]);
       orientation.Weights = ones(size(orientationConst.Weights))*0.25;
       
       qs(count,:) = gik(initialguess,poseConst,orientationConst,jointConst);
       initialguess = qs(count,:);
       count = count+1;
   end
end

framesPerSecond = 5;
r = rateControl(framesPerSecond);
for i = 1:count-1
    show(lbr,qs(i,:),'PreservePlot',false);
%     show(lbr,qs(i,:));
    drawnow
    waitfor(r);
end

Answer 1

[Yiping Liu]

You sould not use orientationTarget for link1.

Try something like below (You only need pose and joint constraints)

lbr = importrobot('iiwa14.urdf'); 
lbr.DataFormat = 'row';
initialguess = lbr.homeConfiguration;
gripper = 'iiwa_link_ee_kuka';
link1 = 'iiwa_link_1';
count = 1;
gik = generalizedInverseKinematics('RigidBodyTree',lbr,'ConstraintInputs',{'pose','joint'});
poseConst = constraintPoseTarget(gripper);
poseConst.TargetTransform = trvec2tform([0.5 0.5 0.4])*eul2tform([0 0 -pi]);
jointConst = constraintJointBounds(lbr);
maxJointChange = deg2rad(10);

min = -70;
max = 70;
for i = min:5:max
   if i == min
       
       qs(count,:) = gik(initialguess,poseConst,jointConst);
       initialguess = qs(count,:);
       count = count+1;
   else
       bounds = jointConst.Bounds;
       bounds(1,:) = [initialguess(1) + 0.025, bounds(1,2)];
       jointConst.Bounds = bounds;
       
       qs(count,:) = gik(initialguess,poseConst,jointConst);
       initialguess = qs(count,:);
       count = count+1;
   end
end

for i = 1:count-1
    if i == 1
        ax = show(lbr,qs(i,:),'PreservePlot',false);
        view(ax, 80, 36);
    else
        show(lbr,qs(i,:),'PreservePlot',false, 'Parent', ax);
    end
    drawnow
    hold on
end

[Ranatchai Laosiripong]

Thank you for the answer,

I'd like to control the configuration of the robot like the video. but in this code it seem to be random in configuration, do you have any advice for doing something like that

[Yiping Liu]

why do you say the configurations seem to be random? the animation should look pretty smooth.

[Ranatchai Laosiripong]

Yes, at first it's smooth but after some point the configuration is change suddenly, back and forth. And the answer (qs) from the code is difference when I rerun it.

[Yiping Liu]

I got the same qs when I run the script above, and you can try to visualize that, it should look pretty smooth.

>> qs

qs =

    0.4331    0.9973    0.8355   -0.9656   -0.6823    1.4155   -2.0431
    0.4581    0.9758    0.7628   -0.9656   -0.6200    1.3964   -2.0684
    0.4831    0.9575    0.6943   -0.9656   -0.5620    1.3803   -2.0929
    0.5081    0.9416    0.6290   -0.9656   -0.5073    1.3665   -2.1166
    0.5331    0.9280    0.5663   -0.9656   -0.4552    1.3547   -2.1398
    0.5581    0.9161    0.5056   -0.9656   -0.4052    1.3445   -2.1624
    0.5831    0.9059    0.4465   -0.9656   -0.3570    1.3358   -2.1847
    0.6081    0.8972    0.3887   -0.9656   -0.3101    1.3285   -2.2066
    0.6331    0.8899    0.3321   -0.9656   -0.2644    1.3223   -2.2282
    0.6581    0.8838    0.2763   -0.9656   -0.2196    1.3172   -2.2496
    0.6831    0.8789    0.2212   -0.9656   -0.1756    1.3131   -2.2707
    0.7081    0.8751    0.1667   -0.9656   -0.1322    1.3099   -2.2918
    0.7331    0.8724    0.1125   -0.9656   -0.0892    1.3077   -2.3127
    0.7581    0.8708    0.0586   -0.9656   -0.0464    1.3063   -2.3335
    0.7831    0.8702    0.0048   -0.9656   -0.0038    1.3058   -2.3543
    0.8081    0.8706   -0.0489   -0.9656    0.0387    1.3061   -2.3751
    0.8331    0.8721   -0.1028   -0.9656    0.0814    1.3074   -2.3959
    0.8581    0.8746   -0.1569   -0.9656    0.1244    1.3094   -2.4168
    0.8831    0.8782   -0.2113   -0.9656    0.1678    1.3124   -2.4378
    0.9081    0.8828   -0.2663   -0.9656    0.2117    1.3164   -2.4590
    0.9331    0.8887   -0.3220   -0.9656    0.2563    1.3213   -2.4803
    0.9581    0.8958   -0.3785   -0.9656    0.3018    1.3273   -2.5019
    0.9831    0.9043   -0.4360   -0.9656    0.3484    1.3344   -2.5237
    1.0081    0.9142   -0.4948   -0.9656    0.3964    1.3429   -2.5459
    1.0331    0.9257   -0.5552   -0.9656    0.4460    1.3527   -2.5685
    1.0581    0.9390   -0.6175   -0.9656    0.4977    1.3642   -2.5916
    1.0831    0.9545   -0.6823   -0.9656    0.5519    1.3776   -2.6152
    1.1081    0.9723   -0.7502   -0.9656    0.6093    1.3933   -2.6395
    1.1331    0.9932   -0.8220   -0.9656    0.6707    1.4118   -2.6647

If you check the solver parameter,

>> gik.SolverParameters

ans = 

  struct with fields:

         MaxIterations: 1500
               MaxTime: 10
     GradientTolerance: 1.0000e-07
     SolutionTolerance: 1.0000e-06
    EnforceJointLimits: 1
    AllowRandomRestart: 1
         StepTolerance: 1.0000e-14

You can see the AllowRandomRestart is turned on by default. You can turn it off - that will guarantee repeatable results, however, in this case, for the given problem, the solver should always be able to comfortably find a solution before it needs to do a random restart.

Which version of MATLAB do you use?

[Ranatchai Laosiripong]

Your qs is running quite smooth like you said.

I use 2020b version. It's really weird that I get different result from you.

[Yiping Liu]

I just tested the script in 2020b release and I get exactly the same result shown above. Maybe some internal files in your MATLAB installation got corrupted? I would suggest you start a tech support case regarding this issue.