Error with inverseKinematic solver

Hello,

I am using the inverseKinematic solver in a Matlab ROS node. When ever the callback function is called I get an error for "Insufficient number of outputs from the right hand side of equal sign to satisfy assignment."

Everything that is passed to the ik solver is printed before hand and everything seems to be okay. Any insight would be greatly appreciated!

ALSO if anyone is more experienced than I am with Matlab and ROS: Is this the proper way to initialize and define publishers that are to publish new data on topics from within a subscriber callback function?

Thank You!

Kyle

My ROS node:

rosinit
%N = robotics.ros.Node("motionPlanner")
%node = robotics.ros.Node('/test');

%global params to be set in callback function
%{
global robot;
global homeConfig;
global ik;
global disPub;
global thetaPub;
global phiPub;
global disMsg;
global thetaMsg;
global phiMsg;
%}
global disPub;
global thetaPub;
global phiPub;
global homeConfig;

robot = importrobot('appleRobot.urdf');
homeConfig = robot.homeConfiguration;
ik = robotics.InverseKinematics('RigidBodyTree', robot);
ik.SolverParameters = struct("AllowRandomRestarts", false);

disPub = rospublisher('/valvControlMsg', 'std_msgs/Float64');
pause(2);
thetaPub = rospublisher('thetaControlMsg', 'std_msgs/Float64');
pause(2);
phiPub = rospublisher('/phiControlMsg', 'std_msgs/Float64');
pause(2);

sub = rossubscriber('/tube_based_positions', ...
    "geometry_msgs/PoseArray", @solvDH);
pause(1);

%Create msgs to publish
disMsg = rosmessage(disPub);
thetaMsg = rosmessage(thetaPub);
phiMsg = rosmessage(phiPub);

solvDH.m

function solvDH(~, tempPos)
    %disp('!!!!!!!!!!!!!!!!!!HERE!!!!!!!!!!!!!! ');
    global disMsg ; global thetaMsg ; global phiMsg ;
    global disPub; global thetaPub; global phiPub;
    global robot; global ik; global homeConfig;
    
    newPos = tempPos.Poses(1).Position;
    
    %newPos = receive(sub, 10);
    x = newPos.X;
    y = newPos.Y;
    z = newPos.Z;

    %adjust x value
    x = x + 22;
    pos = [x y z];

    tform = trvec2tform(pos);
    weights = [0.25 0.25 0.25 1 1 1];

    [configSoln,solnInfo] = ik('end', tform, weights, homeConfig)
    
    dis = 0; theta = 0; phi = 0;
    
    dis = configSoln(1).JointPosition;
    theta = configSoln(2).JointPosition;
    phi = configSoln(3).JointPosition;

    [dis theta phi]
    
    %Populate msgs to Publish
    disMsg.Data = dis;
    thetaMsg.Data = theta;
    phiMsg.Data = phi;

    %Publish
    send(disPub, disMsg); send(thetaPub, thetaMsg);
    send(phiPub, phiMsg);

end

Answer 1

[Kyle Lammers]

Fixed the error. The ik solver should have been global, the call in the subscriber node was a redefinition.

[Cam Salzberger]

The ROS-related code looks mostly fine to me. The only thing I'd recommend is avoiding the use of globals (just generally a good idea to avoid). You can get around it by doing something like this to pass in any variables you need in the callback:

function main
    myPub = rospublisher(topic, type);
    emptyMsg = rosmessage(myPub);
    mySub = rossubscriber(topic, type, @(~, msg) myCB(msg, myPub, emptyMsg))
end
function myCB(subMsg, pub, pubMsg)
    ...
end

For more in-depth reasoning on why to avoid globals, please see here.

-Cam