Inverse Kinematics for a 4-DoF Leg - How to properly limit joint angles and get physical solution

I am trying to apply inverse kinematics to robot leg that has 4 degrees of freedom. I do not know how to think about solving these problems, so any guidance would be greatly appreciated!

The legs are from this system and I am working to understand only one currently (this may be the wrong approach): https://www.robotshop.com/en/lynxmotion-t-hex-4dof-hexapod-robot-kit-hardware-only.html

In MATLAB, I have worked through several examples and I have tried to transition them to the case of this system. I have attached my code so far. Currently, I am struggling with applying joint constraints, getting the configuration that is more physical, and getting a useable solution from this approach. So my questions to this are:

1. How do joint constraints work in the robotics toolbox?

• It seems like rotation is right-handed with respect to the axis of rotation. In addition, when a positive angle is applied, it seems to rotate the rigidbody below the local coordinate system of the joint. However, there are joint constraints to the servos and when I try to apply these, the IK solver that I setup fails to follow the defined trajectory and will give errors saying that the joint angle is outside of the constraints set, even when I make sure that they are valid ranges. I have not been able to find useful documentation on this.

2. If I get the joint constraints set correctly, will the solution be in the correct configuration?

• Should I consider other constraints to apply?
• Is the invverseKinematics from MATLAB what I need or should I be trying generalizedInverseKinematics?

Follow up questions that I have (may not be within the scope of this question) include:

• Will this approach transition well to when there are more legs (i.e. the complete robot with 6 legs)?
• Currently, I defined some random arc for the trajectory. Should I consider something else for the walk of a hexapedal robot?

I am open to any suggestions since it is clear that I do not have any direction. Please let me know if more information is needed and I will edit the post. Physical dimensions of the robot are included in the code and labled in the attached figure. The height of base is arbitrary currently. I have tried to comment the code heavily so that it is easy to understand. Lastly, here are 4 images showing the current results of the MATLAB simulation. blue lines are the body segments and the black ark is the trajectory.

% Robot_Advanced_Real.m
% Copied from matlab and adapted for my scenario.
% Creates robot, uses IK to calculate positions, Creates animation

% Initialize MATLAB
clear all;
close all;
clc;
tic;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Building Robot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Start with Blank RigidTreeBody Model
robot = rigidBodyTree('DataFormat','column','MaxNumBodies',5);

% Specify Arm Lengths of the Robot Arm. Units: [mm]
L1 = 28.45;
L2 = 75.41;
L3 = 70.02;
L4 = 87.14;

% Height of robot body off of Ground
G = 100;

% Add first body with joint. Set as robot base.
body = rigidBody('link1');
joint1 = rigidBodyJoint('joint1', 'revolute');
setFixedTransform(joint1,trvec2tform([0 0 G]));
joint1.JointAxis = [0 0 1];
joint1.HomePosition = pi/6;
% joint.PositionLimits = [0, pi];
body.Joint = joint1;
addBody(robot, body, 'base');

% Add second body with joint. Attach to First
body = rigidBody('link2');
joint2 = rigidBodyJoint('joint2','revolute');
setFixedTransform(joint2, trvec2tform([L1,0,0]));
joint2.JointAxis = [0 1 0];
joint2.HomePosition = pi/6;
% joint.PositionLimits = [0, pi];
body.Joint = joint2;
addBody(robot, body, 'link1');

% Add third body with joint. Attach to second
body = rigidBody('link3');
joint3 = rigidBodyJoint('joint3','revolute');
setFixedTransform(joint3, trvec2tform([L2,0,0]));
joint3.JointAxis = [0 1 0];
joint3.HomePosition = pi/6;
% joint.PositionLimits = [0, pi];
body.Joint = joint3;
addBody(robot, body, 'link2');

% Add fourth body with joint. Attach to third
body = rigidBody('link4');
joint4 = rigidBodyJoint('joint4','revolute');
setFixedTransform(joint4, trvec2tform([L3,0,0]));
joint4.JointAxis = [0 1 0];
joint4.HomePosition = pi/6;
% joint.PositionLimits = [0, pi];
body.Joint = joint4;
addBody(robot, body, 'link3');

% Add End Effector with Fixed Joint. Attach to final body.
body = rigidBody('tool');
joint = rigidBodyJoint('fix1','fixed');
setFixedTransform(joint, trvec2tform([L4, 0, 0]));
body.Joint = joint;
addBody(robot, body, 'link4');

% View Robot Input details to review setup
showdetails(robot);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Define Trajectory
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% Define series of [x y z] for arc step.
% Define parameters of the arc.
xCenter = 0;
yCenter = 120;
zCenter = 0;
xRadius = 70;
yRadius = 20;
zRadius = 20;

% Define the angle theta as going from 30 to 150 degrees in 100 steps.
theta = linspace(30, 150, 100);
% Define x and y using "Degrees" version of sin and cos.
x = (xRadius * cosd(theta) + xCenter)'; 
y = (yRadius * sind(theta) + yCenter)'; 
z = (zRadius * sind(theta) + zCenter)';

% Points around circle
% points = center + [x y z];
points = [x y z];
count  = length(x);

% Check
% plot3(points(:,1),points(:,2),points(:,3))
plot3(x,y,z)
hold on
plot3(points(:,1),points(:,2),points(:,3),'k')
xlabel('x')
ylabel('y')
zlabel('z')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Inverse Kinematics
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Use IK object to find a solution to the joint configurations that achieve
% end effector coordinates along ttrajectory

% Pre-allocate configuration solutions as a matrix, qs
q0 = homeConfiguration(robot);
ndof = length(q0);
qs = zeros(count, ndof);

% create the IK Solver.
ik = inverseKinematics('RigidBodyTree', robot);

% Use desired weights for solution (First three are orientation, last three are translation)
% Since it is a 4-DOF robot with only one revolute joint in Z we do not
% put a weight on Z rotation; otherwise it limits the solution space
% weights = [0, 0, 0, 1, 1, 0];
weights = [0.1, 0.1, 0, 1, 1, 1];
endEffector = 'tool';

% Loop through the trajectory and store the configurations. IK is used to
% give joint configurations.
qInitial = q0; % Use home configuration as the initial guess
for i = 1:count
    % Solve for the configuration satisfying the desired end effector
    % position
    point = points(i,:);
    qSol = ik(endEffector,trvec2tform(point),weights,qInitial);
    % Store the configuration
    qs(i,:) = qSol;
    % Start from prior solution
    qInitial = qSol;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Animate Solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Show first configuration. Show trajectory.
figure
show(robot,qs(1,:)');
ax = gca;
ax.Projection = 'orthographic';
hold on
plot3(points(:,1),points(:,2),points(:,3),'k')
view([-37.5 30])

% Set up animation. Show robot in each configuration on trajectory
framesPerSecond = 7;
r = rateControl(framesPerSecond);
for i = 1:count
    show(robot,qs(i,:)','PreservePlot',false);
    drawnow
    waitfor(r);
end

toc

[Jake Keiper]

Karsh,

Thanks for responding. What do you mean by my reproducible? Once I figure this out, I will reach out to Tech Support.

Also, are their any guides/tutorials on transitioning towards a full robot body?

[Karsh Tharyani]

Hi Jake,

I meant a code snippet/script which can reproduce the error that you quoted in your post:

"However, there are joint constraints to the servos and when I try to apply these, the IK solver that I setup fails to follow the defined trajectory and will give errors saying that the joint angle is outside of the constraints set"

To your second question - did you mean tutorials on modeling a hexapedal robot? We currently have tutorials limited to manipulators with a fixed base. It would be great if you could provide some details of your use-case to our Technical Support team so that we can assist you better.

Best,

Karsh

Answer 1

[Karsh Tharyani]

Hi Jake,

Thank you for your questions.

1. I think you don't have the reproducible with the consraintJointBounds throwing the aforementioned error. EnforceJointLimits ensures that joint limits are considered during evaluation of the solutoin. Please reach out to Technical Support https://www.mathworks.com/support/contact_us.html with your reproducible that shows the error being thrown. This will help us resolve your query faster.
2. generalizedInverseKinematics should be used if you have additonal constraints(position, orientation, aiming, etc.) amongst a subset of your bodies on top of an end-effector pose target. From your description, it seems the only constraint that you have is on joint limits. That should be respected by inverseKinematics.

Hope that helps,

Karsh