%% Generate Joint Positions to Achieve End-Effector Position
% Generate joint positions for a robot model to achieve a desired end-effector position. The inverseKinematics system object uses inverse kinematic algorithms to solve for valid joint positions.
% Load example robots. The puma1 robot is a rigidBodyTree model of a six-axis robot arm with six revolute joints.

load exampleRobots.mat
showdetails(puma1)

%
% Generate a random configuration. Get the transformation from the end effector (L6) to the base for that random configuration. Use this transform as a goal pose of the end effector. Show this configuration.

randConfig = puma1.randomConfiguration;
tform = getTransform(puma1,randConfig,'L6','base');

show(puma1,randConfig);

%
% Create an inverseKinematics object for the puma1 model. Specify weights for the different components of the pose. Use a lower magnitude weight for the orientation angles than the position components. Use the home configuration of the robot as an initial guess.

ik = inverseKinematics('RigidBodyTree',puma1);
weights = [0.25 0.25 0.25 1 1 1];
initialguess = puma1.homeConfiguration;

%
% Calculate the joint positions using the ik object.

[configSoln,solnInfo] = ik('L6',tform,weights,initialguess);

%
% Show the newly generated solution configuration. The solution is a slightly different joint configuration that achieves the same end-effector position. Multiple calls to the ik object can give similar or very different joint configurations.

show(puma1,configSoln);

%
% Copyright 2019 The MathWorks, Inc.